﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Windows.Forms;
using Microsoft.Kinect;
using System.Drawing;
using System.Drawing.Imaging;
using System.Runtime.InteropServices;

namespace Final_Project
{
    // Class manages all direct actions to kinect, start, stop and routing color and depth info to Gui and KinectAlg.
    public class CKinectMngr
    {
        private CGui m_cGui;
        private CAbsFusionStrategy m_cStrategy;

        public KinectSensor m_cSensor;
        private DepthImagePixel[] m_cDepthPixelDataWrite;
        private short[] m_cDepthShortDataWrite;
        private byte[] m_pbColorPixelData;
        private bool m_bEnableKinectFusionAlg = false;
        private IntPtr colorPtr;

        [STAThread]
        static void Main()
        {
            // **************** Test Code
            CFusionFileStrategy cf = new CFusionFileStrategy();
            cf.RunAlg();
            // ************************
            /*
            Application.EnableVisualStyles();
            Application.SetCompatibleTextRenderingDefault(false);
            Application.Run(new CGui());
            */
        }

        public CKinectMngr()
        {
            // read from xml file strategy type
            m_cStrategy = new CFusionFileStrategy();
            // read size from xml
            m_cDepthPixelDataWrite = new DepthImagePixel[76800];
            m_cDepthShortDataWrite = new short[76800];

            m_cSensor = KinectSensor.KinectSensors.First();
            m_cSensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
            m_cSensor.ColorFrameReady += new EventHandler<ColorImageFrameReadyEventArgs>(sensor_ColorFrameReady);
            m_cSensor.DepthStream.Enable(DepthImageFormat.Resolution320x240Fps30);
            m_cSensor.DepthFrameReady += new EventHandler<DepthImageFrameReadyEventArgs>(sensor_DepthFrameReady);

            colorPtr = Marshal.AllocHGlobal(2);
        }

        public void Start()
        {
            m_bEnableKinectFusionAlg = true;
        }

        public void Stop()
        {
            m_bEnableKinectFusionAlg = false;
            m_cStrategy.RunAlg();
        }

        public void StartSensor()
        {
            m_cSensor.Start();
        }

        public void StopSensor()
        { 
            m_cSensor.Stop();
        }

        /*
        private Bitmap DepthToBitmap(DepthImageFrame depthImageFrame)
        {
            IntPtr depthPtr;

            m_pshDepthPixelData = new short[depthImageFrame.PixelDataLength];
            depthImageFrame.CopyPixelDataTo(m_pshDepthPixelData);

            Bitmap bmap = new Bitmap(depthImageFrame.Width, depthImageFrame.Height, PixelFormat.Format16bppRgb555);

            BitmapData bmapdata = bmap.LockBits(new Rectangle(0, 0, depthImageFrame.Width, depthImageFrame.Height), ImageLockMode.WriteOnly, bmap.PixelFormat);
            depthPtr = bmapdata.Scan0;
            Marshal.Copy(m_pshDepthPixelData, 0, depthPtr, depthImageFrame.Width * depthImageFrame.Height);
            bmap.UnlockBits(bmapdata);
            return bmap;
        } // DepthToBitmap */

        private Bitmap ColorToBitmap(ColorImageFrame colorImageFrame)
        {
            if (null == m_pbColorPixelData)
            {
                m_pbColorPixelData = new byte[colorImageFrame.PixelDataLength];  
            }
            Marshal.FreeHGlobal(colorPtr);
            colorPtr = Marshal.AllocHGlobal(m_pbColorPixelData.Length);
            colorImageFrame.CopyPixelDataTo(m_pbColorPixelData); 
            Marshal.Copy(m_pbColorPixelData, 0, colorPtr, m_pbColorPixelData.Length);
            Bitmap bmap = new Bitmap(colorImageFrame.Width, colorImageFrame.Height, colorImageFrame.Width * colorImageFrame.BytesPerPixel, PixelFormat.Format32bppRgb, colorPtr);
            
            return bmap;
        } // ColorToBitmap

        private void sensor_DepthFrameReady(object sender, DepthImageFrameReadyEventArgs e)
        {

            using (DepthImageFrame DFrame = e.OpenDepthImageFrame())
            {
                if (null != DFrame)
                {
                    //m_cGui.UpdatePictureBox(DepthToBitmap(DFrame));
                    if (true == m_bEnableKinectFusionAlg)
                    {
                        DFrame.CopyDepthImagePixelDataTo(m_cDepthPixelDataWrite);
                        DFrame.CopyPixelDataTo(m_cDepthShortDataWrite);
                        m_cStrategy.HandleNewDepthMap(m_cDepthPixelDataWrite, m_cDepthShortDataWrite);
                    }
                }
            }
        } // sensor_DepthFrameReady

        private void sensor_ColorFrameReady(object sender, ColorImageFrameReadyEventArgs e)
        {
            using (ColorImageFrame CFrame = e.OpenColorImageFrame())
            {
                if (null != CFrame)
                {
                    m_cGui.UpdatePictureBox(ColorToBitmap(CFrame));
                }
            }
        } // sensor_ColorFrameReady

        public void SetCGui(CGui cGui)
        {
            m_cGui = cGui;
        }       
    }
}
